#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include "axisgroup/axisgroup_robot.h"
#include "axisgroup/pfsm_servo.h"
#include "axisgroup/pfsm_servo_filter.h"
#include "axis/axis.h"
#include "axisgroup/axisgroup_set.h"
#include "axisgroup/rt_servo_plan.h"
#include "kine/kine.h"

#include <thread>

#define TEST_COUNT 1000

using namespace axisgroup;

class MockAxis : public axis::Axis
{
public:
    MOCK_METHOD0(run_state, RunState());
    MOCK_METHOD0(get_id, int());
};

class ServoModeTest : public ::testing::Test
{
protected:
    virtual void SetUp() override
    {
        for (int i = 0; i < 7; i++) { EXPECT_CALL(joint[i], get_id()).WillRepeatedly(testing::Return(i)); }
    }
    virtual void TearDown() override {}

    MockAxis joint[7];
    pid_t pid_;
    int shm_id_ = -1;
};

static void check_pose(ZucPose pos1, ZucPose pos2)
{
    ASSERT_NEAR(pos1.tran.x, pos2.tran.x, 1e-6);
    ASSERT_NEAR(pos1.tran.y, pos2.tran.y, 1e-6);
    ASSERT_NEAR(pos1.tran.z, pos2.tran.z, 1e-6);
    ASSERT_NEAR(pos1.a, pos2.a, 1e-6);
    ASSERT_NEAR(pos1.b, pos2.b, 1e-6);
    ASSERT_NEAR(pos1.c, pos2.c, 1e-6);
}

static void check_jpos(RobJointVal pos1, RobJointVal pos2, double err = 1e-6)
{
    for (size_t i = 0; i < RobJointVal::MAX_AXIS; i++) { ASSERT_NEAR(pos1[i], pos2[i], err); }
}

TEST_F(ServoModeTest, RtPlanServo)
{
    auto kine = kine::create_kine(axisgroup::RobotSetIntf::get_module_type(12100001));
    // 初始化DH参数
    double a[6] = {0, 0, 0.595, 0.5715, 0, 0};
    double d[6] = {0.14265, 0, 0, -0.1315, 0.115, 0.1035};
    double alpha[6] = {0, M_PI / 2, 0, 0, M_PI / 2, -M_PI / 2};
    double offset[6] = {};
    RobotDH dh = {};
    for (int i = 0; i < 6; i++)
    {
        dh.a[i] = a[i] * 1000.0;
        dh.d[i] = d[i] * 1000.0;
        dh.alpha[i] = alpha[i];
        dh.joint_homeoff[i] = offset[i];
    }
    kine::KineItf::Robot_Config rdh;
    rdh.robot_dh = dh;
    kine->init(rdh);

    auto rt_servo = std::make_shared<RtPlanServo>(0);
    AxisGroupPose init_pos;
    ZucPose tool = {};
    ZucPose frame = {};
    kine->fkine(init_pos.jpos, init_pos.pose, &tool, &frame);
    rt_servo->init(kine, init_pos, 0.008);

    AxisGroupPose cmd;
    AxisGroupPose desired;
    AxisGroupPose revised;
    for (int i = 0; i < 6; i++) { cmd.jpos[i] = i; }
    ASSERT_NE(rt_servo->push_cmd(0, 1, cmd), 0);  // 未开启不能设置

    rt_servo->set_mode(axisgroup::RtPlanServo::ServoMode::SERVO_JOINT);
    rt_servo->set_active(true);

    // servoj 运动
    ASSERT_EQ(rt_servo->push_cmd(1, 1, cmd), 0);  // 绝对运行
    rt_servo->update(desired, revised);
    check_jpos(cmd.jpos, revised.jpos);

    ASSERT_EQ(rt_servo->push_cmd(0, 1, cmd), 0);  // 相对运动
    rt_servo->update(desired, revised);
    for (int i = 0; i < 6; i++) { cmd.jpos[i] *= 2; }
    check_jpos(cmd.jpos, revised.jpos);

    for (int i = 0; i < 6; i++) { cmd.jpos[i] = 90; }
    ASSERT_EQ(rt_servo->push_cmd(1, 1, cmd), 0);  // 绝对运行
    rt_servo->update(desired, revised);
    check_jpos(cmd.jpos, revised.jpos);

    rt_servo->set_active(false);

    kine->fkine(cmd.jpos, cmd.pose, &tool, &frame);
    rt_servo->init(kine, cmd, 0.008);
    rt_servo->set_mode(axisgroup::RtPlanServo::ServoMode::SERVO_COORD);  // 切换为coord模式
    rt_servo->set_active(true);

    cmd.pose.tran.x += 20;
    ASSERT_EQ(rt_servo->push_cmd(1, 1, cmd), 0);  // 绝对运动 笛卡尔
    rt_servo->update(desired, revised);
    check_pose(revised.pose, cmd.pose);

    AxisGroupPose rel;
    rel.pose.tran.x = 20;
    ASSERT_EQ(rt_servo->push_cmd(0, 1, rel), 0);  // 相对运动 笛卡尔
    rt_servo->update(desired, revised);
    cmd.pose.tran.x += rel.pose.tran.x;
    check_pose(revised.pose, cmd.pose);
}

// 测试滤波
TEST_F(ServoModeTest, RtPlanFilter)
{
    auto kine = kine::create_kine(axisgroup::RobotSetIntf::get_module_type(12100001));
    // 初始化DH参数
    double a[6] = {0, 0, 0.595, 0.5715, 0, 0};
    double d[6] = {0.14265, 0, 0, -0.1315, 0.115, 0.1035};
    double alpha[6] = {0, M_PI / 2, 0, 0, M_PI / 2, -M_PI / 2};
    double offset[6] = {};
    RobotDH dh = {};
    for (int i = 0; i < 6; i++)
    {
        dh.a[i] = a[i] * 1000.0;
        dh.d[i] = d[i] * 1000.0;
        dh.alpha[i] = alpha[i];
        dh.joint_homeoff[i] = offset[i];
    }
    kine::KineItf::Robot_Config rdh;
    rdh.robot_dh = dh;
    kine->init(rdh);

    auto rt_servo = std::make_shared<RtPlanServo>(0);
    AxisGroupPose init_pos;
    ZucPose tool = {};
    ZucPose frame = {};
    kine->fkine(init_pos.jpos, init_pos.pose, &tool, &frame);
    rt_servo->init(kine, init_pos, 0.008);

    rt_servo->servo_filter_->set_filter_type(axisgroup::ServoFilterIntf::FilterType::LPF_1ST);
    double vel[6] = {100, 100, 100, 100, 100, 100};
    double acc[6] = {100, 100, 100, 100, 100, 100};
    rt_servo->servo_filter_->init_servo_filter(6, &init_pos, vel, acc, 0.001);
    rt_servo->servo_filter_->set_lpf_para(10);

    rt_servo->set_mode(axisgroup::RtPlanServo::ServoMode::SERVO_JOINT);
    rt_servo->set_active(true);
    for (int i = 0; i < 6; i++) { init_pos.jpos[i] = i * 10; }
    rt_servo->push_cmd(1, 1, init_pos);
    AxisGroupPose target;
    for (int i = 0; i < TEST_COUNT; i++)
    {
        rt_servo->update(init_pos, target);
        // printf("Now %d, %s\n", i, target.jpos.val_str().c_str());
    }
    check_jpos(init_pos.jpos, target.jpos, 1e-3);
}
